import java.awt.Point;
import java.awt.geom.Point2D;
import java.awt.geom.Point2D.Double;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import java.util.Stack;

/**
 * The path planning module for the system. An instance of this class is
 * responsible for identifying a suitable position on the map to explore. Then
 * plan a path from the robot position to this suitable position. The path
 * planning is done using the WaveFront algorithm and suitable goal positions
 * are found by frontier based exploration.
 * 
 * @author Emanuel Yifter Lindgren (dv11elr), Anders Lumio (dv11alo)
 *
 */
public class FrontierPathPlanner implements Planner {
    Chartographer cGrapher;
    public static final int EMPTY_PIXEL_VAL = 0;
    public static final int UNKNOWN_PIXEL_VAL = 4;
    public static final int OBSTACLE_PIXEL_VAL = 15;
    public static final int GROWTH_DEPTH_LIMIT = 4;
    public static final double ESTIMATED_ROBOT_SIZE = 0.6;
    public int[][] internalMap;
    public int internalXAxis;
    public int internalYAxis;
    private final int mapScale;
    public final int[][] nb = { { -1, 0 }, { 0, -1 }, { 0, 1 }, { 1, 0 },
            { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } };

    public FrontierPathPlanner(final Chartographer cGrapher) {
        this.cGrapher = cGrapher;

        internalMap = cGrapher.getMap();
        internalXAxis = internalMap.length;
        internalYAxis = internalMap[0].length;
        mapScale = cGrapher.getInternalMapScale();

    }

    /*
     * (non-Javadoc)
     * 
     * @see Planner#plan()
     */
    @Override
    public List<Point2D.Double> plan() {
        final int[] robotPos = cGrapher.getRobotInternalPosition();
        internalMap = cGrapher.getMap();
        internalXAxis = internalMap.length;
        internalYAxis = internalMap[0].length;

        growObstacles(internalMap, internalMap, robotPos);

        final List<List<Point>> frontiers = getFrontiers(internalMap);

        // sort the frontiers by score.
        Collections.sort(frontiers, new FrontierComparator(robotPos));

        // We search for an optimal path to the best possible frontier.
        List<Point> pathToFrontier = null;
        for (final List<Point> front : frontiers) {
            pathToFrontier = searchOptimalPathToFrontier(
                    getEntryPoint(front, robotPos), internalMap, robotPos);
            if (pathToFrontier != null) {
                break;
            }
        }

        final List<Double> pathToFrontierRealWorldCoords = cGrapher
                .convertInternalToRealWorld(pathToFrontier);

        return pathToFrontierRealWorldCoords;
    }

    /**
     * Calculates and returns a list of all frontiers from the internal map.
     * 
     * @param map
     *            the internal map representation.
     * @return a list of frontiers
     */
    private List<List<Point>> getFrontiers(final int[][] map) {

        final HashMap<String, Point> borderPointLookup = new HashMap<String, Point>();
        final Stack<Point> borderPoints = new Stack<Point>();
        final List<List<Point>> ret = new LinkedList<List<Point>>();

        for (int x = 0; x < internalXAxis; x++) {
            for (int y = 0; y < internalYAxis; y++) {
                // We're interested in a cell if it's an unknown with a known
                // empty neighbor. This means its a border cell.
                if (map[x][y] == UNKNOWN_PIXEL_VAL) {
                    for (int nOffset = 0; nOffset < nb.length; nOffset++) {
                        final int neighborX = x + nb[nOffset][0];
                        final int neighborY = y + nb[nOffset][1];
                        if (neighborX < 0 || neighborX > internalXAxis - 1
                                || neighborY > internalYAxis - 1
                                || neighborY < 0) {
                            continue;
                        }
                        if (map[neighborX][neighborY] == EMPTY_PIXEL_VAL) {
                            // the unknown cell has a known empty neighbor
                            final Point cellPos = new Point(x, y);

                            borderPointLookup.put(x + "x" + y + "y", cellPos);
                            borderPoints.add(cellPos);
                            break;
                        }
                    }
                }

            }
        }

        if (borderPoints.empty()) {
            // no bordering cells found.
            return ret;
        }
        // build chains of unknown points, a.k.a frontiers.
        while (!borderPoints.empty()) {
            final Point startCell = borderPoints.pop();
            if (borderPointLookup.containsKey(startCell.x + "x" + startCell.y
                    + "y")) {
                final LinkedList<Point> frontier = new LinkedList<Point>();
                searchChain(startCell, frontier, borderPointLookup);
                ret.add(frontier);
            }
        }

        return ret;
    }

    /**
     * Constructs frontiers by taking a border position and adding neighboring
     * border positions to a list. This is done recursively until no bordering
     * neighbors exist for the given position. The final list contains a chain
     * of border positions, a.k.a a frontier.
     * 
     * @param pos
     *            a position on the map.
     * @param frontier
     *            the frontier list
     * @param borderPointLookup
     *            a lookup which contains all known border positions.
     */
    private void searchChain(final Point pos, final LinkedList<Point> frontier,
            final HashMap<String, Point> borderPointLookup) {

        frontier.add(pos);
        // we remove the border point after its been added since it can not be
        // part of multiple frontiers.
        borderPointLookup.remove(pos.x + "x" + pos.y + "y");
        for (int nOffset = 0; nOffset < nb.length; nOffset++) {
            final int neighborX = pos.x + nb[nOffset][0];
            final int neighborY = pos.y + nb[nOffset][1];
            if (borderPointLookup
                    .containsKey(neighborX + "x" + neighborY + "y")) {
                searchChain(borderPointLookup.get(neighborX + "x" + neighborY
                        + "y"), frontier, borderPointLookup);
            }
        }
    }

    /**
     * Gets an "acceptable" entry point on a frontier which should then be used
     * to path plan to. The middle point is preferred however if it's too close
     * to the robot a random point is selected. This is necessary as
     * complications can occur if the mid point is underneath the robot.
     * 
     * @param front
     *            the front to select an entry point for.
     * @param robotPosition
     *            the position of the robot.
     * @return a point on the front which can be used as a goal in the path
     *         planning.
     */
    private Point getEntryPoint(final List<Point> front,
            final int[] robotPosition) {

        final Random rand = new Random();
        final int size = front.size();
        Point entryPoint = front.get(size / 2);
        double distScore = calcDistance(entryPoint, robotPosition);
        // we don't want a point that is too close to(under) robot.
        if (distScore < ESTIMATED_ROBOT_SIZE * mapScale) {
            do {
                final int rIndex = rand.nextInt(size);
                entryPoint = front.get(rIndex);
                distScore = calcDistance(front.get(rIndex), robotPosition);
            } while (distScore < ESTIMATED_ROBOT_SIZE * mapScale);
        }
        return entryPoint;

    }

    /**
     * Calculates the distance between an internal point coordinate and the
     * robots internal position.
     * 
     * @param point
     *            an internal point
     * @param robotPosition
     *            the internal position of the robot
     * @return the distance
     */
    private double calcDistance(final Point point, final int[] robotPosition) {

        final double distance = Math.sqrt(Math.pow(robotPosition[0] - point.x,
                2) + Math.pow(robotPosition[1] - point.y, 2));
        return distance;
    }

    /**
     * @param frontPoint
     *            A point on the frontier, the goal point.
     * @param map
     *            the internal map.
     * @param robotPosition
     *            the position of the robot on the internal map.
     * @return an optimal path from the robot to the front/goal point.
     */
    private List<Point> searchOptimalPathToFrontier(final Point frontPoint,
            final int[][] map, final int[] robotPosition) {
        if (frontPoint == null) {
            return null;
        }
        List<Point> pathToFrontPoint = null;
        final int[][] wavefrontMap = new int[internalXAxis][internalYAxis];

        for (int x = 0; x < internalXAxis; x++) {
            for (int y = 0; y < internalYAxis; y++) {
                if (internalMap[x][y] != EMPTY_PIXEL_VAL) {
                    wavefrontMap[x][y] = 1;
                }
            }
        }

        final LinkedList<Point> fringe = new LinkedList<Point>();
        fringe.add(new Point(frontPoint.x, frontPoint.y));
        wavefrontMap[frontPoint.x][frontPoint.y] = 2;
        propagateWaveFrontBFS(wavefrontMap, fringe);

        pathToFrontPoint = calculatePath(wavefrontMap, robotPosition);

        return pathToFrontPoint;
    }

    /**
     * Calculates/Walks an optimal path from robot position to goal position
     * using a wavefrontMap.
     * 
     * @param wavefrontMap
     *            the wavefront map.
     * @param robotPosition
     *            the position of the robot.
     * @return an optimal path from the robot to the goal.
     */
    private List<Point> calculatePath(final int[][] wavefrontMap,
            final int[] robotPosition) {
        final LinkedList<Point> ret = new LinkedList<Point>();
        ret.add(new Point(robotPosition[0], robotPosition[1]));

        int pX = robotPosition[0];
        int pY = robotPosition[1];
        while (wavefrontMap[pX][pY] != 2) {
            final int pathLength = ret.size();
            for (int nOffset = 0; nOffset < nb.length; nOffset++) {

                final int neighborX = pX + nb[nOffset][0];
                final int neighborY = pY + nb[nOffset][1];
                if (neighborX < 0 || neighborX > internalXAxis - 1
                        || neighborY < 0 || neighborY > internalYAxis - 1) {
                    continue;
                }
                if (wavefrontMap[neighborX][neighborY] == (wavefrontMap[pX][pY] - 1)
                        || wavefrontMap[neighborX][neighborY] == 2) {
                    ret.add(new Point(neighborX, neighborY));
                    pX = neighborX;
                    pY = neighborY;
                    break;
                }
            }
            // dead end exit
            if (pathLength == ret.size()) {
                return null;
            }
        }

        return ret;
    }

    /**
     * Grows obstacles on our internal map. This is done to handle potential
     * inaccuracy of sensor data and to force the planner not to plan paths too
     * close to real world obstacles.
     * 
     * @param outputMap
     *            The output map the has grown obstacles.
     * @param srcMap
     *            the src map which has not had its obstacles grown.
     * @param robotposition
     *            the position of the robot on the map. Needed to prevent
     *            growing over it.
     */
    private void growObstacles(final int[][] outputMap, final int[][] srcMap,
            final int[] robotposition) {

        final List<Point> obstacles = new LinkedList<Point>();

        for (int x = 0; x < internalXAxis; x++) {
            for (int y = 0; y < internalYAxis; y++) {
                if (srcMap[x][y] > UNKNOWN_PIXEL_VAL + 1) {
                    obstacles.add(new Point(x, y));
                }
            }
        }
        for (final Point obstacle : obstacles) {
            grow(obstacle.x, obstacle.y, outputMap, robotposition, 0);
        }

    }

    private void grow(final int obstacleX, final int obstacleY,
            final int[][] wavefrontMap, final int[] robotposition,
            final int depth) {

        if (GROWTH_DEPTH_LIMIT - depth == 0) {
            return;
        }

        for (int nOffset = 0; nOffset < nb.length; nOffset++) {

            final int neighborX = obstacleX + nb[nOffset][0];
            final int neighborY = obstacleY + nb[nOffset][1];
            if (neighborX < 0 || neighborX > internalXAxis - 1 || neighborY < 0
                    || neighborY > internalYAxis - 1) {
                continue;
            }
            // we dont allow growth too close to robot position.
            if (calcDistance(new Point(neighborX, neighborY), robotposition) > ESTIMATED_ROBOT_SIZE
                    * mapScale) {
                grow(neighborX, neighborY, wavefrontMap, robotposition,
                        depth + 1);
            }
        }

        wavefrontMap[obstacleX][obstacleY] = 1;

    }

    /**
     * Creates a wavefront map by BFS.
     * 
     * @param map
     *            the wavefront map to fill with values.
     * @param fringe
     *            a list of surrounding neighbor nodes. Initialized with the
     *            goal point.
     */
    private void propagateWaveFrontBFS(final int[][] map,
            final List<Point> fringe) {
        while (!fringe.isEmpty()) {
            final Point node = fringe.remove(0);

            // find children/neighbors
            for (int nOffset = 0; nOffset < nb.length; nOffset++) {

                final int neighborX = node.x + nb[nOffset][0];
                final int neighborY = node.y + nb[nOffset][1];
                if (neighborX < 0 || neighborX > internalXAxis - 1
                        || neighborY < 0 || neighborY > internalYAxis - 1) {
                    continue;
                }
                if (map[neighborX][neighborY] == 0
                        && map[neighborX][neighborY] != 1) {
                    map[neighborX][neighborY] = map[node.x][node.y] + 1;
                    fringe.add(new Point(neighborX, neighborY));
                }
            }

        }
    }

    /**
     * An implementation of a Comparator. The class is used to sort a list of
     * frontiers by their score. The score is derived from
     * (FrontierSize-distanceToFrontier*DISTANCE_WEIGHT) and naturally a higher
     * score is preferred.
     * 
     * @author Emanuel Yifter Lindgren (dv11elr), Anders Lumio (dv11alo)
     *
     */
    public class FrontierComparator implements Comparator<List<Point>> {
        private final static int DISTANCE_WEIGHT = 3;
        private final int[] robotPosition = new int[2];

        public FrontierComparator(final int[] robotPosition) {
            this.robotPosition[0] = robotPosition[0];
            this.robotPosition[1] = robotPosition[1];
        }

        @Override
        public int compare(final List<Point> f1, final List<Point> f2) {

            int f1Score = f1.size();
            int f2Score = f2.size();

            f1Score += (int) ((-1 * calcDistance(
                    getEntryPoint(f1, robotPosition), robotPosition)) * DISTANCE_WEIGHT);
            f2Score += (int) ((-1 * calcDistance(
                    getEntryPoint(f2, robotPosition), robotPosition)) * DISTANCE_WEIGHT);

            // -1* because we want the reverse-natural
            // ordering(greatest->lowest)
            return -1 * (f1Score - f2Score);
        }
    }
}
